import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
from turtlesim.msg import Pose
import tf_transformations
class TFDynamicBroadcastPyNode(Node):
    def __init__(self):
        super().__init__('TF_dynamic_broadcast_py_node')
        self.get_logger().info('TFDynamicBroadcastPyNode创建')
        self.broadcast: TransformBroadcaster =TransformBroadcaster(self)
        self.sub=self.create_subscription(Pose,"/turtle1/pose",self.do_pose,10)
    def do_pose(self,pose:Pose):
        ts: TransformStamped=TransformStamped()
        ts.header.frame_id="world1"
        ts.header.stamp=self.get_clock().now().to_msg();
        #
        ts.child_frame_id="turtle1"
        #
        ts.transform.translation.x=pose.x
        ts.transform.translation.y=pose.y
        ts.transform.translation.z=0.0
        qtn: tuple=tf_transformations.quaternion_from_euler(0.0,0.0,pose.theta)
        ts.transform.rotation.x=qtn[0]
        ts.transform.rotation.y=qtn[1]
        ts.transform.rotation.z=qtn[2]
        ts.transform.rotation.w=qtn[3]
        #  x, y, z 是相对于世界坐标系 world1的。​
        #  这表示：turtle1坐标系的原点相对于 world1坐标系的位置偏移量。

        self.broadcast.sendTransform(ts)
def main():
    rclpy.init()
    rclpy.spin(TFDynamicBroadcastPyNode())
    rclpy.shutdown()


if __name__ == '__main__':
    main()